﻿using System;
using System.Collections.Generic;
using System.Linq;
using System.Text;
using System.Threading.Tasks;


namespace ConsoleApp
{
    public static class GeodeticUtils1
    {
        // WGS84 椭球参数
        private const double EquatorialRadius = 6378137.0;
        private const double Flattening = 1.0 / 298.257223563;
        private const double EccentricitySquared = 2 * Flattening - Flattening * Flattening;

        /// <summary>
        /// 高精度将经纬度转换为ECEF坐标 (WGS84)
        /// </summary>
        public static (double X, double Y, double Z) GeodeticToEcef(double lat, double lon, double alt)
        {
            double latRad = DegreeToRadian(lat);
            double lonRad = DegreeToRadian(lon);

            double n = EquatorialRadius / Math.Sqrt(1 - EccentricitySquared * Math.Sin(latRad) * Math.Sin(latRad));
            double x = (n + alt) * Math.Cos(latRad) * Math.Cos(lonRad);
            double y = (n + alt) * Math.Cos(latRad) * Math.Sin(lonRad);
            double z = (n * (1 - EccentricitySquared) + alt) * Math.Sin(latRad);

            return (x, y, z);
        }

        /// <summary>
        /// 高精度将ECEF坐标转换为经纬度 (Vincenty方法)
        /// </summary>
        public static (double Lat, double Lon, double Alt) EcefToGeodetic(double x, double y, double z)
        {
            const double tolerance = 1e-12;
            const int maxIterations = 100;

            double p = Math.Sqrt(x * x + y * y);
            double lon = Math.Atan2(y, x);

            // 初始估计
            double lat = Math.Atan2(z, p * (1.0 - EccentricitySquared));
            double alt = 0;
            double n = EquatorialRadius;

            for (int i = 0; i < maxIterations; i++)
            {
                double sinLat = Math.Sin(lat);
                double prevLat = lat;

                n = EquatorialRadius / Math.Sqrt(1 - EccentricitySquared * sinLat * sinLat);
                alt = p / Math.Cos(lat) - n;

                lat = Math.Atan2(z + n * EccentricitySquared * sinLat, p);

                if (Math.Abs(lat - prevLat) < tolerance) break;
            }

            return (RadianToDegree(lat), RadianToDegree(lon), alt);
        }

        /// <summary>
        /// 高精度将ENU坐标转换为ECEF坐标
        /// </summary>
        public static (double X, double Y, double Z) EnuToEcef(
            double east, double north, double up,
            double refLat, double refLon, double refAlt)
        {
            (double refX, double refY, double refZ) = GeodeticToEcef(refLat, refLon, refAlt);

            double latRad = DegreeToRadian(refLat);
            double lonRad = DegreeToRadian(refLon);

            double sinLat = Math.Sin(latRad);
            double cosLat = Math.Cos(latRad);
            double sinLon = Math.Sin(lonRad);
            double cosLon = Math.Cos(lonRad);

            // 旋转矩阵元素
            double t11 = -sinLon;
            double t12 = -sinLat * cosLon;
            double t13 = cosLat * cosLon;

            double t21 = cosLon;
            double t22 = -sinLat * sinLon;
            double t23 = cosLat * sinLon;

            double t31 = 0;
            double t32 = cosLat;
            double t33 = sinLat;

            double dx = t11 * east + t12 * north + t13 * up;
            double dy = t21 * east + t22 * north + t23 * up;
            double dz = t31 * east + t32 * north + t33 * up;

            return (refX + dx, refY + dy, refZ + dz);
        }

        /// <summary>
        /// 构建高精度旋转矩阵 (使用四元数避免万向节锁)
        /// </summary>
        public static double[,] CreateRotationMatrix(double yawDeg, double pitchDeg, double rollDeg)
        {
            double yaw = DegreeToRadian(NormalizeAngle(yawDeg));
            double pitch = DegreeToRadian(NormalizeAngle(pitchDeg));
            double roll = DegreeToRadian(NormalizeAngle(rollDeg));

            // 使用四元数避免万向节锁
            double cy = Math.Cos(yaw * 0.5);
            double sy = Math.Sin(yaw * 0.5);
            double cp = Math.Cos(pitch * 0.5);
            double sp = Math.Sin(pitch * 0.5);
            double cr = Math.Cos(roll * 0.5);
            double sr = Math.Sin(roll * 0.5);

            // 四元数
            double w = cr * cp * cy + sr * sp * sy;
            double x = sr * cp * cy - cr * sp * sy;
            double y = cr * sp * cy + sr * cp * sy;
            double z = cr * cp * sy - sr * sp * cy;

            // 从四元数转换为旋转矩阵
            double xx = x * x;
            double xy = x * y;
            double xz = x * z;
            double xw = x * w;

            double yy = y * y;
            double yz = y * z;
            double yw = y * w;

            double zz = z * z;
            double zw = z * w;

            return new double[,]
            {
                { 1 - 2 * (yy + zz),     2 * (xy - zw),     2 * (xz + yw) },
                {     2 * (xy + zw), 1 - 2 * (xx + zz),     2 * (yz - xw) },
                {     2 * (xz - yw),     2 * (yz + xw), 1 - 2 * (xx + yy) }
            };
        }

        /// <summary>
        /// 计算两点间的大地距离 (Vincenty公式)
        /// </summary>
        public static double CalculateGeodeticDistance(double lat1, double lon1, double lat2, double lon2)
        {
            const double a = EquatorialRadius;
            const double b = a * (1 - Flattening);
            const double f = Flattening;

            double L = DegreeToRadian(lon2 - lon1);
            double U1 = Math.Atan((1 - f) * Math.Tan(DegreeToRadian(lat1)));
            double U2 = Math.Atan((1 - f) * Math.Tan(DegreeToRadian(lat2)));

            double sinU1 = Math.Sin(U1);
            double cosU1 = Math.Cos(U1);
            double sinU2 = Math.Sin(U2);
            double cosU2 = Math.Cos(U2);

            double lambda = L;
            double lambdaP = 2 * Math.PI;
            double sinSigma = 0;
            double cosSigma = 0;
            double sigma = 0;
            double cosSqAlpha = 0;
            double cos2SigmaM = 0;

            int iterLimit = 100;
            double sinLambda, cosLambda;

            do
            {
                sinLambda = Math.Sin(lambda);
                cosLambda = Math.Cos(lambda);

                sinSigma = Math.Sqrt(
                    Math.Pow(cosU2 * sinLambda, 2) +
                    Math.Pow(cosU1 * sinU2 - sinU1 * cosU2 * cosLambda, 2)
                );

                if (Math.Abs(sinSigma) < 1e-12) return 0;  // 重合点

                cosSigma = sinU1 * sinU2 + cosU1 * cosU2 * cosLambda;
                sigma = Math.Atan2(sinSigma, cosSigma);

                double sinAlpha = cosU1 * cosU2 * sinLambda / sinSigma;
                cosSqAlpha = 1 - sinAlpha * sinAlpha;

                if (Math.Abs(cosSqAlpha) < 1e-12)
                {
                    cos2SigmaM = 0;
                }
                else
                {
                    cos2SigmaM = cosSigma - 2 * sinU1 * sinU2 / cosSqAlpha;
                }

                double C = f / 16 * cosSqAlpha * (4 + f * (4 - 3 * cosSqAlpha));
                lambdaP = lambda;
                lambda = L + (1 - C) * f * sinAlpha *
                    (sigma + C * sinSigma * (cos2SigmaM + C * cosSigma * (-1 + 2 * cos2SigmaM * cos2SigmaM)));

            } while (Math.Abs(lambda - lambdaP) > 1e-12 && --iterLimit > 0);

            if (iterLimit == 0) return double.NaN;  // 公式未收敛

            double uSq = cosSqAlpha * (a * a - b * b) / (b * b);
            double A = 1 + uSq / 16384 * (4096 + uSq * (-768 + uSq * (320 - 175 * uSq)));
            double B = uSq / 1024 * (256 + uSq * (-128 + uSq * (74 - 47 * uSq)));

            double deltaSigma = B * sinSigma *
                (cos2SigmaM + B / 4 *
                 (cosSigma * (-1 + 2 * cos2SigmaM * cos2SigmaM) -
                  B / 6 * cos2SigmaM * (-3 + 4 * sinSigma * sinSigma) * (-3 + 4 * cos2SigmaM * cos2SigmaM)));

            double s = b * A * (sigma - deltaSigma);

            return s;
        }

        public static double DegreeToRadian(double deg) => deg * Math.PI / 180.0;
        public static double RadianToDegree(double rad) => rad * 180.0 / Math.PI;

        public static double NormalizeAngle(double angle)
        {
            // 将角度标准化到[-180, 180]范围
            angle %= 360;
            if (angle > 180) angle -= 360;
            if (angle < -180) angle += 360;
            return angle;
        }
    }

    public class HighPrecisionDroneConverter
    {
        private const double GroundTolerance = 1e-10;
        private const double TerrainHeight = 0.0; // 默认地形高度（可替换为实际地形数据）

        /// <summary>
        /// 高精度计算图像中心的地面坐标
        /// </summary>
        public static (double Lon, double Lat) CalculateImageCenter(
            double droneLon, double droneLat, double droneAlt,
            double yaw, double pitch, double roll)
        {
            // 1. 获取高精度旋转矩阵
            double[,] R = GeodeticUtils.CreateRotationMatrix(yaw, pitch, roll);

            // 2. 计算相机光轴方向
            double[] bodyDirection = { 0, 0, -1 };

            // 3. 转换到ENU坐标系
            double[] enuDirection = {
                R[0,0]*bodyDirection[0] + R[0,1]*bodyDirection[1] + R[0,2]*bodyDirection[2],
                R[1,0]*bodyDirection[0] + R[1,1]*bodyDirection[1] + R[1,2]*bodyDirection[2],
                R[2,0]*bodyDirection[0] + R[2,1]*bodyDirection[1] + R[2,2]*bodyDirection[2]
            };

            // 4. 归一化方向向量
            double norm = Math.Sqrt(
                enuDirection[0] * enuDirection[0] +
                enuDirection[1] * enuDirection[1] +
                enuDirection[2] * enuDirection[2]
            );

            enuDirection[0] /= norm;
            enuDirection[1] /= norm;
            enuDirection[2] /= norm;

            // 5. 处理特殊姿态
            if (Math.Abs(enuDirection[2]) < GroundTolerance)
            {
                // 水平指向时返回无人机正下方位置
                return (droneLon, droneLat);
            }

            // 6. 计算与地面交点（考虑地形高度）
            double heightAboveTerrain = droneAlt - TerrainHeight;
            if (heightAboveTerrain <= 0)
                throw new InvalidOperationException("Drone is below terrain level");

            double t = -heightAboveTerrain / enuDirection[2];
            double east = enuDirection[0] * t;
            double north = enuDirection[1] * t;

            // 7. 高精度转换到地理坐标
            (double ecefX, double ecefY, double ecefZ) =
                GeodeticUtils.EnuToEcef(east, north, TerrainHeight, droneLat, droneLon, 0);

            (double lat, double lon, double _) = GeodeticUtils.EcefToGeodetic(ecefX, ecefY, ecefZ);

            return (lon, lat);
        }

        /// <summary>
        /// 迭代法提高坐标精度（适用于需要毫米级精度的应用）
        /// </summary>
        public static (double Lon, double Lat) CalculateImageCenterIterative(
            double droneLon, double droneLat, double droneAlt,
            double yaw, double pitch, double roll, int iterations = 3)
        {
            // 初始估计
            (double lon, double lat) = CalculateImageCenter(
                droneLon, droneLat, droneAlt, yaw, pitch, roll);
            double alt = 0;
            for (int i = 0; i < iterations; i++)
            {
                // 计算当前估计点与无人机位置的距离和方位
                double distance = GeodeticUtils1.CalculateGeodeticDistance(droneLat, droneLon, lat, lon);
                double azimuth = CalculateAzimuth(droneLat, droneLon, lat, lon);

                // 计算方向向量
                double azRad = GeodeticUtils1.DegreeToRadian(azimuth);
                double east = distance * Math.Sin(azRad);
                double north = distance * Math.Cos(azRad);

                // 重新计算交点
                (double ecefX, double ecefY, double ecefZ) =
                    GeodeticUtils.EnuToEcef(east, north, TerrainHeight, droneLat, droneLon, 0);

                (lat, lon, alt) = GeodeticUtils1.EcefToGeodetic(ecefX, ecefY, ecefZ);
            }

            return (lon, lat);
        }

        /// <summary>
        /// 计算从起点到终点的方位角（正北为0°，顺时针增加）
        /// </summary>
        private static double CalculateAzimuth(double lat1, double lon1, double lat2, double lon2)
        {
            double lat1Rad = GeodeticUtils1.DegreeToRadian(lat1);
            double lon1Rad = GeodeticUtils1.DegreeToRadian(lon1);
            double lat2Rad = GeodeticUtils1.DegreeToRadian(lat2);
            double lon2Rad = GeodeticUtils1.DegreeToRadian(lon2);

            double dLon = lon2Rad - lon1Rad;

            double y = Math.Sin(dLon) * Math.Cos(lat2Rad);
            double x = Math.Cos(lat1Rad) * Math.Sin(lat2Rad) -
                      Math.Sin(lat1Rad) * Math.Cos(lat2Rad) * Math.Cos(dLon);

            double azimuthRad = Math.Atan2(y, x);
            double azimuth = GeodeticUtils1.RadianToDegree(azimuthRad);

            return (azimuth + 360) % 360;
        }
    }

}